
/**
   ********************* (C) COPYRIGHT 2021 LionHeart **********************
   * @file         chassis.c/h 
   * @brief  
   *
   * @note
   * @history
   *  Version      Data             Author
   *  V1.0.0       2021-10-21 23-18 Yanfubing
   *
    @verbatim
    ========================================================================
    
    ========================================================================
    @endverbatim
************************ (C) COPYRIGHT 2021 LionHeart **********************
*/
#include "chassis.h"
#include "main.h"
#include "tim.h"
#include "gpio.h"
#include "pid.h"
#include "math.h"
  //PID 的调节参数
	const double pid_data_init[3] = { MOTOR_SPEED_PID_KP, MOTOR_SPEED_PID_KI, MOTOR_SPEED_PID_KD }; 
	double set_speed=60;             //期望速度，在发车前输入期望时间后动态计算,cm/s
	double motor_speed[1];        //电机转速
	pid_type_def speed_pid[1];    //PID控制数据
	uint16_t start_flag=1;        //标准位
	uint16_t huanchongtime=0;
	double w_speed[1]={0};        //轮速计算的中间变量
	short motorspeed;
	uint16_t pid_out;               //速度的PID输出控制
	uint16_t servo_pwm;            //控制舵机的转向PWM
	uint8_t ccd_speed;             //CCDP_PWM的绝对值
  void chassisInit()
{
	//PID初始化
	 PID_init(&speed_pid[MOTOR_1], PID_POSITION, pid_data_init, MOTOR_SPEED_PID_MAX_OUT, MOTOR_SPEED_PID_MAX_IOUT);
}

/**
 * @description:马达的PID控制&&舵机的转向控制
 * @param     void chassisControlLoop(void)
 * @return    pid_out
 */

void chassisControlLoop(void)
{
	ccd_speed=__fabs(CCD_PWM);
	w_speed[MOTOR_1]  = set_speed+ccd_speed*TURN_SPEED; 
	//PID计算
	//motor_speed: 4069，根据编码器实际接线得出
	/********计算得出电机的PID输出值**********/
	PID_calc(&speed_pid[MOTOR_1], motorspeed, w_speed[MOTOR_1]); 
	
	pid_out=speed_pid[MOTOR_1].out;  //把电机的PID输出给pid_out
    Servo_controller(CCD_PWM);       //舵机控制函数
	
		//发送出速度值，最终的实际速度
	if (set_speed > 0.0f)    //运行中，使用PID控制底盘运行
	{
		pid_out=pid_out;
		HAL_GPIO_WritePin(IN1_GPIO_Port,IN1_Pin,GPIO_PIN_RESET);//控制速度的方向；
    HAL_GPIO_WritePin(IN2_GPIO_Port,IN2_Pin,GPIO_PIN_SET);
		__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, pid_out);
	}
	else        //朝着反方向运动
	{
		pid_out=-pid_out;
		HAL_GPIO_WritePin(IN1_GPIO_Port,IN1_Pin,GPIO_PIN_SET);  //控制速度的反方向；
        HAL_GPIO_WritePin(IN2_GPIO_Port,IN2_Pin,GPIO_PIN_RESET);
		__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, pid_out);  
	}
	
}

/*****************************************************************************/

void Servo_controller(float ccd_pwm)
{  

  if(ccd_pwm>=0)
	{
		ccd_pwm=Kalman_Filter_U(ccd_pwm);
		servo_pwm=INT16_C(-ccd_pwm*50+750); //根据CCD的值计算舵机的舵量
		
	}
	else
	{
		ccd_pwm=Kalman_Filter_U(ccd_pwm);
	   servo_pwm=INT16_C(750-ccd_pwm*50);
		
	}
	if(servo_pwm>1000)servo_pwm=1000;
	if(servo_pwm<=500)servo_pwm=500;
	__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,servo_pwm);
  
}

float Kalman_Filter_U(float val)  //测量电压的卡尔曼滤波
{
    static float Xk1 = 0.1;
    static float eESTK1 = 0.9;           //估计误差
    static float eMEAK1 = 0.2;           //测量误差
    static float Kk1;
    
    Kk1 = eESTK1/(eESTK1 + eMEAK1);
    Xk1 =val;

    eESTK1 = (1 - Kk1) * eESTK1;
    
   return Xk1;
}





/**
* @description:外部中断
 * @param     void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
 * @return    KEY1 KEY2 按键控制初始速度；
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
		switch(GPIO_Pin)
		{
			case KEY0_Pin:  
				start_flag=1;
		   HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
			if(set_speed<60)
			{
			set_speed=60;
			}
			else 
				set_speed+=10;
				if(set_speed>380)
				set_speed=380;

				break;
		  case KEY1_Pin:   
				start_flag=1;
				HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
				set_speed-=10;
				if(set_speed<60)
				set_speed=60;
				break;
		}
}


